#include "nunchuck.h"
#include "i2clib.h"

int16_t normalize(int16_t sens, int16_t middle, int16_t target, int16_t range, int16_t min, int16_t max)
{
  int16_t value;

  value = (int16_t)(((long)((sens-middle))*(long)(target))/range);
  if (value > max) value = max;
  if (value < min) value = min;

  return value;
}


void nunchuckInit()
{
  twi_init(); // initialize i2c

  _delay_ms(10); // wait a bit

  // send init to nunchuck
  uint8_t data[2];

  data[0] = 0x40;
  data[1] = 0x00;
  twi_writeTo(0x52, data, 2, 1);
}

void nunchuck_NextData()
{
  static uint8_t zero[]= {0};
  twi_writeTo(0x52, zero, 1, 1);
}

void nunchuckRead()
{
  // request data
  uint8_t data[6];

  

  twi_readFrom(0x52, data, 6);

  // read data from buffer
  uint8_t i;
  for(i = 0; i < 6; i++)
  {
    sensors[i] = data[i];
  }
  nunchuck_NextData();
  
  
  sensors[0] = (sensors[0] ^ 0x17) + 0x17;
  sensors[1] = (sensors[1] ^ 0x17) + 0x17;

  sensors[5] = (sensors[5] ^ 0x17) + 0x17;
  butZ =  sensors[5] & 0b00000001;
  butC = (sensors[5] & 0b00000010)>>1;

  sensors[2] = ((uint16_t)((sensors[2] ^ 0x17) + 0x17)<<2)|((sensors[5] & 0b00001100)>>2);
  sensors[3] = ((uint16_t)((sensors[3] ^ 0x17) + 0x17)<<2)|((sensors[5] & 0b00110000)>>4);
  sensors[4] = ((uint16_t)((sensors[4] ^ 0x17) + 0x17)<<2)|((sensors[5] & 0b11000000)>>6);
  

  joyX = (int8_t)(normalize(sensors[0], (joyXmax+joyXmin)/2, 127, joyXmax-joyXmin, -127, 127));
  joyY = (int8_t)(normalize(sensors[1], (joyYmax+joyYmin)/2, 127, joyYmax-joyYmin, -127, 127));
  
//  joyX = (int8_t)(sensors[0]);
//  joyY = (int8_t)(sensors[1]);


  accelX = normalize(sensors[2], (accXmax+accXmin)/2, 180, accXmax-accXmin, -90, 90);
//  accelX = (int16_t)(((long)(sensors[2]-accXM)*(long)(accXO)) / accXI);

  accelY = normalize(sensors[3], (accYmax+accYmin)/2, 180L, accYmax-accYmin, -90, 90);
  accelZ = normalize(sensors[4], (accZmax+accZmin)/2, 180L, accZmax-accZmin, -90, 90);


}
